import math
import time

import numpy as np

from controller.PID import PID
from render.TacviewRender import TacviewRender
from Sim.simcore.FlightSim import AircraftSimulator
from Sim.simcore.catalog import Catalog as c, Catalog
from input.JoyStick import FlightJoystick

red_param = {
    Catalog.ic_long_gc_deg: 85.8,  # geodesic longitude [deg]
    Catalog.ic_lat_geod_deg: 9.7,  # geodesic latitude  [deg]
    Catalog.ic_h_sl_ft: 20000,  # altitude above mean sea level [ft]20000
    Catalog.ic_psi_true_deg: 180.0,  # initial (true) heading [deg] (0, 360)
    Catalog.ic_u_fps: 800.0,  # body frame x-axis velocity [ft/s]  (-2200, 2200)
    Catalog.ic_v_fps: 0.0,  # body frame y-axis velocity [ft/s]  (-2200, 2200)
    Catalog.ic_w_fps: 0.0,  # body frame z-axis velocity [ft/s]  (-2200, 2200)
    Catalog.ic_p_rad_sec: 0.0,  # roll rate  [rad/s]  (-2 * pi, 2 * pi)
    Catalog.ic_q_rad_sec: 0.0,  # pitch rate [rad/s]  (-2 * pi, 2 * pi)
    Catalog.ic_r_rad_sec: 0.0,  # yaw rate   [rad/s]  (-2 * pi, 2 * pi)
    Catalog.ic_roc_fpm: 0.0,  # initial rate of climb [ft/min]
    Catalog.ic_terrain_elevation_ft: 0,
}
blue_param = {
    Catalog.ic_long_gc_deg: 85.5,  # geodesic longitude [deg]
    Catalog.ic_lat_geod_deg: 9.0,  # geodesic latitude  [deg]
    Catalog.ic_h_sl_ft: 20000,  # altitude above mean sea level [ft]
    Catalog.ic_psi_true_deg: 0.0,  # initial (true) heading [deg] (0, 360)
    Catalog.ic_u_fps: 800.0,  # body frame x-axis velocity [ft/s]  (-2200, 2200)
    Catalog.ic_v_fps: 0.0,  # body frame y-axis velocity [ft/s]  (-2200, 2200)
    Catalog.ic_w_fps: 0.0,  # body frame z-axis velocity [ft/s]  (-2200, 2200)
    Catalog.ic_p_rad_sec: 0.0,  # roll rate  [rad/s]  (-2 * pi, 2 * pi)
    Catalog.ic_q_rad_sec: 0.0,  # pitch rate [rad/s]  (-2 * pi, 2 * pi)
    Catalog.ic_r_rad_sec: 0.0,  # yaw rate   [rad/s]  (-2 * pi, 2 * pi)
    Catalog.ic_roc_fpm: 0.0,  # initial rate of climb [ft/min]
    Catalog.ic_terrain_elevation_ft: 0,

}

if __name__ == '__main__':
    flight = AircraftSimulator(uid="A0100",
                               color="Red",
                               model='737',
                               init_state={},
                               origin=(120.0, 60.0, 0.0),
                               sim_freq=60, num_missiles=0, default_condition=red_param)
    flight2 = AircraftSimulator(uid="B0100",
                                color="Blue",
                                model='737',
                                init_state={},
                                origin=(120.0, 60.0, 0.0),
                                sim_freq=60, num_missiles=0,default_condition=blue_param)
    target_alt = 6000
    render = TacviewRender()
    render.add_sim(flight)
    render.add_sim(flight2)
    joystick = FlightJoystick()
    pid_controller = PID(P=0.0005, I=0.0000000001, D=0.002)
    pid_controller.setValue = target_alt
    while (True):
        joystick.read_action()
        flight.set_property_value(c.fcs_aileron_cmd_norm, joystick.get_action()[0])
        flight.set_property_value(c.fcs_elevator_cmd_norm, joystick.get_action()[1])
        flight.set_property_value(c.fcs_rudder_cmd_norm, joystick.get_action()[2])
        flight.set_property_value(c.fcs_throttle_cmd_norm, joystick.get_action()[3])
        alt = flight2.get_state()[2]
        out = pid_controller.pidPosition(alt)
        if out > 1:
            out = 1
        if out < -1:
            out = -1
        flight2.set_property_value(c.fcs_aileron_cmd_norm, 0)
        flight2.set_property_value(c.fcs_elevator_cmd_norm, -out)
        flight2.set_property_value(c.fcs_rudder_cmd_norm, 0)
        flight2.set_property_value(c.fcs_throttle_cmd_norm, 0.7)


        flight.run()
        flight2.run()
        # [lon, lat, alt, roll, pitch, yaw]=flight.get_state()
        render.render()
        render.render("realtime")
        render.render("pandas")
        time.sleep(0.0001)
